#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Description:       :使用树莓派串口控制步进电机
@Date     :2023/05/30 09:37:26
@Author      :Cosecant
@version      :1.0
                    _ooOoo_                     
                   o8888888o                    
                  88   .   88                    
                   (| -_- |)                    
                   O\  =  /O                    
                ____/`---'\____                 
              .'   \|     |/   `.               
             /   \|||  :  |||/   \              
            /  _||||| -:- |||||_  \             
            |   | \ \  -  /// |   |             
            | \_|  ''\---/''  |_/ |             
            \  .-\__  `-`  ___/-. /             
          ___`. .'  /--.--\  `. . __            
       .'' '<  `.___\_<|>_/___.'  >' ''.         
      | | :  `- \`.;`\ _ /`;.`/ - ` : | |       
      \  \ `-.   \_ __\ /__ _/   .-` /  /       
 ======`-.____`-.___\_____/___.-`____.-'======   
                    `=---='                     
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^   
        佛祖保佑        永无BUG                  
'''
from myMotor import StepperMotor
import serial
import time
import RPi.GPIO as GPIO

port = '/dev/ttyAMA1'
baudrate = 115200
zero_setted = 0
init_flag = 0
AXIS_MAX_POSITION = 0
#正方向为朝向电机方向


def init_axis_zero(channel, motor: StepperMotor):
    """
    @description  :初始化轴零点，设为光电门正方向180度电机位置
    ---------
    @param  :motor:电机对象,init_flag:初始化标志位(全局变量)
    -------
    """
    global zero_setted, init_flag
    motor.stop()
    if (init_flag == 0):
        print("初始化轴零点")
        time.sleep(0.3)
        motor.position_mode_control_smooth(position=1800,max_speed=1000)
        while motor.read_motor_position_error() > 0.1:
            time.sleep(0.1) #等待电机到达指定位置
        motor.set_zero_position()
        zero_setted = 1
    else:
        print("软件限位与硬件限位冲突")
        exit(1)

    return None


def init_axis_max(channel, motor: StepperMotor):
    """
    @description  :初始化轴最大位置，设为光电门负方向180度电机位置
    ---------
    @param  :motor:电机对象,init_flag:初始化标志位(全局变量),
                AXIS_MAX_POSITION:轴最大位置(全局变量)
    -------
    """
    global AXIS_MAX_POSITION, init_flag
    motor.stop()
    if (init_flag == 0):
        print("初始化轴最大位置")
        AXIS_MAX_POSITION = motor.read_motor_realtime_position() - 180.0
        init_flag = 1
    else:
        print("软件限位与硬件限位冲突")
        exit(1)
    return None


if __name__ == '__main__':
    try:
        mserial = serial.Serial(port, baudrate, timeout=0.5)
    except Exception as e:
        print(e)
        exit()
    testMotor = StepperMotor(mserial, 0x01)
    print(testMotor.read_firmware_and_hardware_version())
    # 设置GPIO模式为BCM
    GPIO.setmode(GPIO.BCM)
    # 设置BCM 17引脚为输入模式，并启用上拉电阻（连接电机远端光电门）
    GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    # 设置BCM 18引脚为输入模式，并启用上拉电阻（连接电机近端光电门）
    GPIO.setup(18, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    init_axis_zero_with_param = lambda channel: init_axis_zero(
        channel, testMotor)
    init_axis_max_with_param = lambda channel: init_axis_max(
        channel, testMotor)
    GPIO.add_event_detect(17,
                          GPIO.RISING,
                          callback=init_axis_zero_with_param,
                          bouncetime=100)
    GPIO.add_event_detect(18,
                          GPIO.RISING,
                          callback=init_axis_max_with_param,
                          bouncetime=100)
    #归零校准
    init_max_speed = 1000
    gate_far = GPIO.input(17)
    gate_near = GPIO.input(18)
    if (gate_far):  #向正方向前进180度
        testMotor.position_mode_control_smooth(position=1800,
                                               max_speed=init_max_speed)
    if (gate_near):  #向负方向前进180度
        testMotor.position_mode_control_smooth(position=-1800,
                                               max_speed=init_max_speed)
    testMotor.speed_mode_control(speed=-500,
                                 slope=1000)  #先向负方向运动直到触发远端光电门并设置零点
    while (zero_setted == 0):
        time.sleep(0.1)
    testMotor.speed_mode_control(speed=500,
                                 slope=1000)  #再向正方向运动直到触发近端光电门并设置最大位置
    while (init_flag == 0):
        time.sleep(0.1)
    testMotor.position_mode_control_smooth(position=3600*3,relative_position=False)
    # 清理GPIO设置
    GPIO.cleanup()